/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Joystick;

/**
 *PLEASE IMPLEMENT ME - REMEMBER , THIS IS A SINGLETON!
 * @author Madison and Christpher
 */
public class Controller {


    //variables relating to drivetrain

    private double maxspeed = 1;
    private boolean shiftLastAttempt = false;
    // joysticks
    private Joystick leftJoystick = new Joystick(1);
    private Joystick rightJoystick = new Joystick(2);
    private Joystick gigaware = new Joystick(3);

    /**
     * A private constructor -- this is a singleton!
     */
    private Controller() {
    }
    private static Controller myInstance;

    public static Controller getInstance() {
        //if this is the first time:
        if (null == myInstance) {
            myInstance = new Controller();
        }
        return myInstance;
    }

    /**
     * checks if buttons are pressed. if they are, it sets fire button to true.
     * not yet implemented
     */
    public boolean fireButton() {
        //TODO get good buttons
        if (leftJoystick.getTrigger() || rightJoystick.getTrigger() || gigaware.getRawButton(1)) {
            return true;
        } else {
            return false;
        }

    }

    /**
     *
     * function not yet implemented
     * @return
     */
    public boolean lowerBridgeArmButton() {
        //TODO
        if (leftJoystick.getRawButton(3) || rightJoystick.getRawButton(3) || gigaware.getRawButton(1)) {
            return true;
        } else {
            return false;
        }
    }

    /**
     * function not yet implemented
     * @return
     */
    public boolean raiseBridgeArmButton() {
        //TODO
        if (leftJoystick.getRawButton(2) || rightJoystick.getRawButton(2)|| gigaware.getRawButton(1)) {
            return true;
        } else {
            return false;
        }
    }

    /**
     * function not yet implemented
     * @return
     */
    public boolean gathererOnbutton() {
        //TODO
        if (rightJoystick.getRawButton(4) || gigaware.getRawButton(1)) {
            return true;
        } else {
            return false;
        }
    }

    /**
     * function not yet implemented
     * @return
     */
    public boolean gathererOffButton() {
        //TODO
        if (leftJoystick.getRawButton(4) || gigaware.getRawButton(1)) {
            return true;
        } else {
            return false;
        }
    }

    /**
     * function not yet implemented
     * @return
     */
    public boolean storageFeederOnButton() {
        //TODO
        if (rightJoystick.getRawButton(4) || rightJoystick.getRawButton(5) || gigaware.getRawButton(1)) {
            return true;
        } else {
            return false;
        }
    }

    /**
     * function not yet implemented
     * @return
     */
    public boolean storageFeederOffButton() {
        //TODO
        if (leftJoystick.getRawButton(4) || leftJoystick.getRawButton(5) || gigaware.getRawButton(1) || gigaware.getRawButton(1)) {
            return true;
        } else {
            return false;
        }
    }

    /**
     * function not yet implemented
     * @return
     */
    public boolean BalanceControlsOn() {
        if (leftJoystick.getRawButton(1) || rightJoystick.getRawButton(1) || gigaware.getRawButton(1)) {
            return true;
        } else {
            return false;
        }
    }

    /**
     * returns slow if the driver is trying to shift to low gear, fast if trying to shift to fast gear
     */
    public boolean Shift(boolean currentGear) {
       
        if (leftJoystick.getRawButton(1)){
            currentGear = false;
        }
        if (rightJoystick.getRawButton(1)) {
            currentGear = true;
        }
        return currentGear;
    }

    /**
     * function not yet implemented
     * will return whether we want to initiate the auto balance routine
     */
    public boolean BalanceControlsOff() {
        //TODO
        if (leftJoystick.getRawButton(1) || rightJoystick.getRawButton(1) || gigaware.getRawButton(1)) {
            return true;
        } else {
            return false;
        }
    }

    /**
     * returns the left speed we send to the motors
     */
    public double GetLeftSpeed(double curve, double deadband) {
        if (Math.abs(leftJoystick.getY()) > deadband) {
            return curve * leftJoystick.getY();
        } else {
            return 0;
        }
    }

    /**
     * returns the right speed we send to the motors
     * @return
     */
    public double GetRightSpeed(double curve, double deadband) {
        if (Math.abs(rightJoystick.getY()) > deadband) {
            return curve * rightJoystick.getY();
        } else {
            return 0;
        }
    }

    /**
     * returns the maximum forward speed
     */
    public double maxSpeed() {
        return maxspeed;
    }

    /**
     * 
     * returns the backwards maximum speed
     */
    public double backwardsMaxSpeed() {
        return -1 * maxspeed;
    }

    //returns the current curve
    public double getCurve() {
        return leftJoystick.getZ();
    }

    //Returns the current deadband value
    public double getDeadband() {
        return rightJoystick.getZ();
    }

    //not yet implemented will return
    public boolean getPIDEnabled() {
        if (leftJoystick.getRawButton(10) || rightJoystick.getRawButton(10) || gigaware.getRawButton(1)) {
            return true;
        } else return false;
        }


    /**allows the use of a toggle button. the last attempt
     * makes it so update can't change it multiple times per button press
     */
    public boolean getToggleButton(Joystick Joystick, int button, boolean lastAttempt) {
   if (Joystick.getRawButton(button) && ! lastAttempt){
       lastAttempt = true;
       return true;
       }else if (Joystick.getRawButton(button) && lastAttempt) {
       lastAttempt = true;
       return false;
   }else if (! Joystick.getRawButton(button)) {
       
       lastAttempt = false;
       return false;  
   } else return false;
      
      }
              
      
    }

